package RKUjava.math;
import RKUjava.util.RKUStringUtils;

/** This class implements a number of useful geometric formulas. The methods assumes that the given vectors
    are "error-free", i.e. no error checking are performed. For all methods involving more than one vector
    the method assumes that the vectors have the same length.
    The extra assumptions are described at each method. */
public class RKUGeometry
{

  /** The angle between two normalized vectors. <br><b>Assumptions:</b>
      <ul>
      <li> Vectors are normalized.
      <li> Vectors are non-zero.
      </ul>
  */
  public static double angleBetweenNormVectors(double[] v1, double[] v2)
    {
      return Math.acos(dotProduct(v1, v2));
    }

  /** The angle between two vectors. <br><b>Assumptions:</b>
      <ul>
      <li> Vectors are non-zero.
      </ul>
  */
  public static double angleBetweenVectors(double[] v1, double[] v2)
    {
      return Math.acos(dotProduct(v1,v2)/(lengthOfVector(v1)*lengthOfVector(v2)));
    }

  /** Normalize a vector. Since arrays are passed by reference the vector v1 contains the result.
      <br><b>Assumptions:</b>
      <ul>
      <li> Vector is non-zero.
      </ul>
  */
  public static void normalizeVector(double[] v1)
    {
      double len = lengthOfVector(v1);

      for (int i=0; i<v1.length;i++) {
	v1[i] = v1[i]/len;
      }
    }

  /** Clone and normalize a vector. 
      <br><b>Assumptions:</b>
      <ul>
      <li> Vector is non-zero.
      </ul>
  */
  public static double[] cloneAndNormalizeVector(double[] v1)
    {
      double[] res = new double[v1.length];
      double len = lengthOfVector(v1);

      for (int i=0; i<v1.length;i++) {
	res[i] = v1[i]/len;
      }
      return res;
    }


  /** The dotproduct of two vectors.  */
  public static double dotProduct(double[] v1, double[] v2)
    {
      double res = 0;
      for (int i=0; i<v1.length;i++) {
	res += v1[i]*v2[i];
      }
      return res;
    }

  /** The length of a vector.  */
  public static double lengthOfVector(double[] v1)
    {
      return Math.sqrt(dotProduct(v1, v1));
    }

  /** The distance between two points.  */
  public static double distancePoint2Point(double[] p1, double[] p2)
    {
      double res = 0;
      for (int i=0; i<p1.length;i++) {
	res += (p1[i]-p2[i])*(p1[i]-p2[i]);
      }
      return Math.sqrt(res);
    }

  /** Tests wether two points are the same.  */
  public static boolean isSamePoint(double[] p1, double[] p2)
    {
      for (int i=0; i<p1.length;i++) {
	if (p1[i]!=p2[i])
	  return false;
      }
      return true;
    }

  /** Add p2 to p1. The result is stored in p1. 
      @param p1 The point to store the result in.
      @param p2 The point to add to p1.
  */
  public static void addPoint2Point(double[] p1, double[] p2)
    {
      for (int i=0;i<p1.length;i++) {
	p1[i] += p2[i];
      }
    }

  /** Add p1 to p2 and store the result in <tt>result</tt>.
      @param p1 The first point.
      @param p2 The second point.
      @param result The array to store the result in.
  */
  public static void addPoint2Point(double[] p1, double[] p2, double[] result)
    {
      for (int i=0;i<p1.length;i++) {
	result[i] = p1[i] + p2[i];
      }
    }

  /** Add p1 to p2 and return the result as a new array. 
      @param p1 The first point.
      @param p2 The second point.
      @return p1+p2 added entry-wise.
  */
  public static double[] addPoint2PointClone(double[] p1, double[] p2)
    {
      double[] res = new double[p1.length];

      for (int i=0;i<p1.length;i++) {
	res[i] = p1[i] + p2[i];
      }
      return res;
    }

  /** Sub p2 from p1. The result is stored in p1. 
      @param p1 The point to store the result in.
      @param p2 The point to subtract from p1.
  */
  public static void subPointFromPoint(double[] p1, double[] p2)
    {
      for (int i=0;i<p1.length;i++) {
	p1[i] -= p2[i];
      }
    }

  /** Sub p2 from p1 and store the result in <tt>result</tt>.
      @param p1 The first point.
      @param p2 The second point.
      @param result The array to store the result in (p1-p2).
  */
  public static void subPointFromPoint(double[] p1, double[] p2, double[] result)
    {
      for (int i=0;i<p1.length;i++) {
	result[i] = p1[i] - p2[i];
      }
    }

  /** Sub p2 from p1 and return the result as a new array. 
      @param p1 The first point.
      @param p2 The second point.
      @return p1-p2 subtracted entry-wise.
  */
  public static double[] subPointFromPointClone(double[] p1, double[] p2)
    {
      double[] res = new double[p1.length];

      for (int i=0;i<p1.length;i++) {
	res[i] = p1[i] - p2[i];
      }
      return res;
    }

  /** Scale a point. Each entry in <tt>p</tt> is multiplied with the
      scale value.
      @param p The point to scale.
      @param scale The scale value.
  */
  public static void scalePoint(double[] p, double scale)
    {
      for (int i=0;i<p.length;i++) {
	p[i] *= scale;
      }
    }

  /** Scale a point and return the scaled point as an array. <tt>p</tt> is
      not altered by this method.
      @param p The point to scale.
      @param scale The scale value.  
      @return The scaled point.
  */
  public static double[] scalePointClone(double[] p, double scale)
    {
      double[] res = new double[p.length];
      for (int i=0;i<p.length;i++) {
	res[i] = p[i] * scale;
      }
      return res;
    }

  /** Set a point to be the zero-vector. Each entry in <tt>p</tt> is 
      set to zero.
      @param p The point to zero.
  */
  public static void zeroPoint(double[] p)
    {
      for (int i=0;i<p.length;i++) {
	p[i] = 0;
      }
    }

}


